#include "../include/libv4l2/taiic_v4l2.h"
#include "../include/libv4l2/taiic_v4l2_config.h"
#include <iostream>
#include "rk_debug.h"

static void errno_exit(const char *s)
{
    std::cerr << s << " error: " << errno << ", " << strerror(errno) << std::endl;
    std::exit(EXIT_FAILURE);
}
static int xioctl(int fd, int request, void *arg)
{
    int r;
    do
    {
        r = ioctl(fd, request, arg);
    } while (r == -1 && errno == EINTR);
    return r;
}

TaiicV4l2::TaiicV4l2()
{
    width = CONF_PIC_W;
    height = CONF_PIC_H;
    format = CONF_PIC_FORMAT;
    n_buffers = CONF_BUFFER_N;
    right_dev_name = CONF_RIGHT_DEV; // 右侧鱼眼相机
    left_dev_name = CONF_LEFT_DEVL;  // 左侧鱼眼相机

    type = (v4l2_buf_type)CONF_DEVICETYPE;
    device_type = CONF_DEVICETYPE;
    memoey_type = CONF_MEMORY;
    plane_n = CONF_PLANE_N;
    RK_LOGD("====config success====\n");
}

void TaiicV4l2::setWidth(int widthSet)
{
    width = widthSet;
}
void TaiicV4l2::setHeight(int heightSet)
{
    height = heightSet;
}
void TaiicV4l2::setFormat(int formatSet)
{
    format = formatSet;
}
void TaiicV4l2::setBuffersN(int n_buffersSet)
{
    n_buffers = n_buffersSet;
}

void TaiicV4l2::setRightDevName(char *right_dev_nameSet)
{
    right_dev_name = right_dev_nameSet;
}
void TaiicV4l2::setLeftDevName(char *left_dev_nameSet)
{
    left_dev_name = left_dev_nameSet;
}
void TaiicV4l2::setDeviceType(int device_typeSet)
{
    type = (v4l2_buf_type)device_typeSet;
    device_type = device_typeSet;
}
void TaiicV4l2::setMemoryType(int memoey_typeSet)
{
    memoey_type = memoey_typeSet;
}
void TaiicV4l2::setPlaneN(int plane_nSet)
{
    plane_n = plane_nSet;
}

// void TaiicV4l2::viFrameCallback(tViFrameDataCallback dataCallback)
// {
//     right_viFrameDataCallback = dataCallback;
// }

void TaiicV4l2::openDevice()
{
    // Open the device
    right_fd = open(right_dev_name, O_RDWR | O_NONBLOCK, 0);
    left_fd = open(left_dev_name, O_RDWR | O_NONBLOCK, 0);
    if (right_fd == -1 || left_fd == -1)
        errno_exit("open");
}
void TaiicV4l2::queDeviceCap()
{
    CLEAR(cap);
    if (xioctl(right_fd, VIDIOC_QUERYCAP, &cap) == -1)
    {
        if (errno == EINVAL)
        {
            std::cerr << right_dev_name << " is not a V4L2 device" << std::endl;
        }
        else
        {
            errno_exit("VIDIOC_QUERYCAP");
        }
    }

    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE))
    {
        std::cerr << right_dev_name << " does not support V4L2_CAP_VIDEO_CAPTURE_MPLANE" << std::endl;
        std::exit(EXIT_FAILURE);
    }
}
void TaiicV4l2::setDeviceFormat()
{
    CLEAR(fmt);
    fmt.type = device_type;
    fmt.fmt.pix_mp.width = width;
    fmt.fmt.pix_mp.height = height;
    fmt.fmt.pix_mp.pixelformat = format;
    fmt.fmt.pix_mp.num_planes = plane_n;
    if (xioctl(right_fd, VIDIOC_S_FMT, &fmt) == -1)
    {
        errno_exit("RIGHT_VIDIOC_S_FMT");
    }

    if (xioctl(left_fd, VIDIOC_S_FMT, &fmt) == -1)
    {
        errno_exit("LEFT_VIDIOC_S_FMT");
    }
}
void TaiicV4l2::allocateBuffer()
{
    CLEAR(req);
    req.type = device_type;
    req.memory = memoey_type;
    req.count = n_buffers;
    if (xioctl(right_fd, VIDIOC_REQBUFS, &req) == -1)
    {
        errno_exit("RIGHT_VIDIOC_REQBUFS");
    }
    if (xioctl(left_fd, VIDIOC_REQBUFS, &req) == -1)
    {
        errno_exit("LEFT_VIDIOC_REQBUFS");
    }
    rightBuffers = new buffer[req.count];
    leftBuffers = new buffer[req.count];

    for (int i = 0; i < n_buffers; ++i)
    {
        // v4l2_plane planes;
        CLEAR(planes);
        planes.length = fmt.fmt.pix_mp.plane_fmt[0].sizeimage;
        planes.m.mem_offset = i * planes.length;

        CLEAR(rightBuf);
        rightBuf.type = req.type;
        rightBuf.memory = req.memory;
        rightBuf.index = i;
        rightBuf.m.planes = &planes;
        rightBuf.length = n_buffers;
        if (xioctl(right_fd, VIDIOC_QUERYBUF, &rightBuf) == -1)
        {
            errno_exit("RIGHT_VIDIOC_QUERYBUF");
        }
        rightBuffers[i].length = rightBuf.m.planes->length;
        rightBuffers[i].start = (unsigned char *)mmap(NULL,
                                                      rightBuffers[i].length,
                                                      PROT_READ | PROT_WRITE, MAP_SHARED, right_fd,
                                                      rightBuf.m.planes->m.mem_offset);

        CLEAR(leftBuf);
        leftBuf.type = req.type;
        leftBuf.memory = req.memory;
        leftBuf.index = i;
        leftBuf.m.planes = &planes;
        leftBuf.length = n_buffers;

        if (xioctl(left_fd, VIDIOC_QUERYBUF, &leftBuf) == -1)
        {
            errno_exit("LEFT_VIDIOC_QUERYBUF");
        }
        leftBuffers[i].length = leftBuf.m.planes->length;
        leftBuffers[i].start = (unsigned char *)mmap(NULL,
                                                     leftBuffers[i].length,
                                                     PROT_READ | PROT_WRITE, MAP_SHARED, left_fd,
                                                     leftBuf.m.planes->m.mem_offset);

        if (rightBuffers[i].start == MAP_FAILED)
        {
            errno_exit("right mmap");
        }
        if (leftBuffers[i].start == MAP_FAILED)
        {
            errno_exit("left mmap");
        }
    }
}
void TaiicV4l2::queBuffer()
{
    for (int i = 0; i < n_buffers; ++i)
    {

        CLEAR(planes);
        planes.length = fmt.fmt.pix_mp.plane_fmt[0].sizeimage;
        planes.m.mem_offset = i * planes.length;

        CLEAR(rightBuf);
        rightBuf.type = req.type;
        rightBuf.memory = req.memory;
        rightBuf.index = i;
        rightBuf.m.planes = &planes;
        rightBuf.length = n_buffers;
        if (xioctl(right_fd, VIDIOC_QBUF, &rightBuf) == -1)
        {
            errno_exit("RIGHE1_VIDIOC_QBUF");
        }

        CLEAR(leftBuf);
        leftBuf.type = req.type;
        leftBuf.memory = req.memory;
        leftBuf.index = i;
        leftBuf.m.planes = &planes;
        leftBuf.length = n_buffers;
        if (xioctl(left_fd, VIDIOC_QBUF, &leftBuf) == -1)
        {
            errno_exit("LEFT1_VIDIOC_QBUF");
        }
    }
}

void TaiicV4l2::initDevice()
{
    openDevice();
    queDeviceCap();
    setDeviceFormat();
    allocateBuffer();
    queBuffer();
}

void TaiicV4l2::threadFunction()
{
    while (is_right_capture && is_left_capture)
    {
        fd_set right_fds;
        FD_ZERO(&right_fds);
        FD_SET(right_fd, &right_fds);
        struct timeval tv;
        tv.tv_sec = 2;
        tv.tv_usec = 0;
        int right_r = select(right_fd + 1, &right_fds, NULL, NULL, &tv);
        if (right_r == -1)
        {
            if (errno == EINTR)
            {
                continue;
            }
            errno_exit("select");
        }
        if (right_r == 0)
        {
            std::cerr << "right select timeout" << std::endl;
            std::exit(EXIT_FAILURE);
        }

        rightBuf.type = req.type;
        rightBuf.memory = req.memory;

        if (xioctl(right_fd, VIDIOC_DQBUF, &rightBuf) == -1)
        {
            if (errno == EAGAIN)
            {
                continue;
            }
            errno_exit("VIDIOC_DQBUF");
        }
        rightTs.tv_sec = rightBuf.timestamp.tv_sec * 1000000000;

        // rightTs.tv_sec = buf.timestamp.tv_sec;

        rightTs.tv_nsec = rightBuf.timestamp.tv_usec * 1000;
        // std::cout << countPic << " timestamp: "<< rightTs.tv_sec << "." << rightTs.tv_nsec << std::endl;
        // std::cout << countPic << " timestamp: "<< rightTs.tv_sec + rightTs.tv_nsec << std::endl;

        if (viFrameDataCallback != NULL)
        {
            viFrameDataCallback("right", rightBuffers[rightBuf.index].start, rightBuffers[rightBuf.index].length, (rightTs.tv_sec + rightTs.tv_nsec));
        }
        if (xioctl(right_fd, VIDIOC_QBUF, &rightBuf) == -1)
        {
            errno_exit("RIGHT_VIDIOC_QBUF");
        }

        //left 
        fd_set left_fds;
        FD_ZERO(&left_fds);
        FD_SET(left_fd, &left_fds);
        struct timeval leftTv;
        leftTv.tv_sec = 2;
        leftTv.tv_usec = 0;
        int left_r = select(left_fd + 1, &left_fds, NULL, NULL, &leftTv);
        if (left_r == -1)
        {
            if (errno == EINTR)
            {
                continue;
            }
            errno_exit("left select");
        }
        if (left_r == 0)
        {
            std::cerr << "left select timeout" << std::endl;
            std::exit(EXIT_FAILURE);
        }

        leftBuf.type = req.type;
        leftBuf.memory = req.memory;

        if (xioctl(left_fd, VIDIOC_DQBUF, &leftBuf) == -1)
        {
            if (errno == EAGAIN)
            {
                continue;
            }
            errno_exit("LEFT_VIDIOC_DQBUF");
        }
        leftTs.tv_sec = leftBuf.timestamp.tv_sec * 1000000000;
        // rightTs.tv_sec = buf.timestamp.tv_sec;
        leftTs.tv_nsec = leftBuf.timestamp.tv_usec * 1000;
        // std::cout << countPic << " timestamp: "<< rightTs.tv_sec << "." << rightTs.tv_nsec << std::endl;
        // std::cout << countPic << " timestamp: "<< rightTs.tv_sec + rightTs.tv_nsec << std::endl;

        if (viFrameDataCallback != NULL)
        {
            viFrameDataCallback("left", leftBuffers[leftBuf.index].start, leftBuffers[leftBuf.index].length, (leftTs.tv_sec + leftTs.tv_nsec));
        }
        if (xioctl(left_fd, VIDIOC_QBUF, &leftBuf) == -1)
        {
            errno_exit("LEFT_VIDIOC_QBUF");
        }
    }
}

// void TaiicV4l2::getFrame()
// {
//     std::memset(&rightTs, 0, sizeof(rightTs));
//     std::memset(&leftTs, 0, sizeof(leftTs));

//     std::thread getFrameData(&TaiicV4l2::threadFunction, this);
//     getFrameData.join();
// }

void TaiicV4l2::startCapt(tViFrameDataCallback dataCallback)
{
    if (xioctl(right_fd, VIDIOC_STREAMON, &type) == -1)
    {
        errno_exit("RIGHT_VIDIOC_STREAMON");
    }
    is_right_capture = true;
    viFrameDataCallback = dataCallback;

    if (xioctl(left_fd, VIDIOC_STREAMON, &type) == -1)
    {
        errno_exit("LEFT_VIDIOC_STREAMON");
    }
    is_left_capture = true;

    getFrame();
}


void TaiicV4l2::stopCapt()
{
    if (xioctl(right_fd, VIDIOC_STREAMOFF, &type) == -1)
    {
        errno_exit("RIGHT_VIDIOC_STREAMOFF");
    }
    is_right_capture = false;

    if (xioctl(left_fd, VIDIOC_STREAMOFF, &type) == -1)
    {
        errno_exit("RIGHT_VIDIOC_STREAMOFF");
    }
    is_left_capture = false;
}

void TaiicV4l2::closeDevice()
{
    for (int i = 0; i < n_buffers; ++i)
    {
        if (munmap(rightBuffers[i].start, rightBuffers[i].length) == -1)
        {
            errno_exit("right munmap");
        }

        if (munmap(leftBuffers[i].start, leftBuffers[i].length) == -1)
        {
            errno_exit("left munmap");
        }
    }

    delete[] rightBuffers;
    delete[] leftBuffers;
    if (close(right_fd) == -1 || close(left_fd) == -1)
    {
        errno_exit("right or left close");
    }
}

void RingBuf_Taiic::initRingbuffer()
{
    if (pHead == NULL)
    {
        pHead = (char *)malloc(buffer_size);
    }
    pValid = pValidTail = pHead;
    pTail = pHead + buffer_size;
    validLen = 0;
}

long RingBuf_Taiic::writeRingbuffer(char *buffer, long addLen)
{
    if (addLen > buffer_size)
        return -2;
    if (pHead == NULL)
        return -1;
    assert(buffer);

    // 将要存入的数据copy到pValidTail处
    if (pValidTail + addLen > pTail) // 需要分成两段copy
    {
        int len1 = pTail - pValidTail;
        int len2 = addLen - len1;
        memcpy(pValidTail, buffer, len1);
        memcpy(pHead, buffer + len1, len2);
        pValidTail = pHead + len2; // 新的有效数据区结尾指针
    }
    else
    {
        memcpy(pValidTail, buffer, addLen);
        pValidTail += addLen; // 新的有效数据区结尾指针
    }

    // 需重新计算已使用区的起始位置
    if (validLen + addLen > buffer_size)
    {
        int moveLen = validLen + addLen - buffer_size; // 有效指针将要移动的长度
        if (pValid + moveLen > pTail)                  // 需要分成两段计算
        {
            int len1 = pTail - pValid;
            int len2 = moveLen - len1;
            pValid = pHead + len2;
        }
        else
        {
            pValid = pValid + moveLen;
        }
        validLen = buffer_size;
    }
    else
    {
        validLen += addLen;
    }

    return 0;
}

long RingBuf_Taiic::readRingbuffer(char *buffer, long len)
{
    if (pHead == NULL)
        return -1;

    assert(buffer);

    if (validLen == 0)
        return 0;

    if (len > validLen)
        len = validLen;

    if (pValid + len > pTail) // 需要分成两段copy
    {
        int len1 = pTail - pValid;
        int len2 = len - len1;
        memcpy(buffer, pValid, len1);       // 第一段
        memcpy(buffer + len1, pHead, len2); // 第二段，绕到整个存储区的开头
        pValid = pHead + len2;              // 更新已使用缓冲区的起始
    }
    else
    {
        memcpy(buffer, pValid, len);
        pValid = pValid + len; // 更新已使用缓冲区的起始
    }
    validLen -= len; // 更新已使用缓冲区的长度
    return len;
}

/*
 * function:获取已使用缓冲区的长度
 * return  :已使用的buffer长度
 * */
long RingBuf_Taiic::getRingbufferValidLen(void)
{
    return validLen;
}

/** function:释放环形缓冲区**/
void RingBuf_Taiic::releaseRingbuffer(void)
{
    if (pHead != NULL)
        free(pHead);
    pHead = NULL;
}
